/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoysticks;

/**
 *
 * @author Roksbot
 */
public class DriveTrain extends Subsystem {
    
    RobotDrive drive;
    CANJaguar frontLeftJaguar,
         rearLeftJaguar,
         frontRightJaguar,
         rearRightJaguar ;
    public void initDefaultCommand() {
        setDefaultCommand(new DriveWithJoysticks());
    }
    public DriveTrain(){
        
        try{
            frontLeftJaguar = new CANJaguar(RobotMap.frontLeftJaguarID);
            rearLeftJaguar = new CANJaguar(RobotMap.rearLeftJaguarID);
            frontRightJaguar = new CANJaguar(RobotMap.frontRightJaguarID);
            rearRightJaguar = new CANJaguar(RobotMap.rearRightJaguarID);
            
            drive = new RobotDrive(frontLeftJaguar,rearLeftJaguar,frontRightJaguar,rearRightJaguar);
            drive.setSafetyEnabled(false);
        } catch (Exception e){
            System.out.println(e);
    }
        
    }
    
    
    public void driveFoward(double speed, Gyroscope gyro){
        drive.mecanumDrive_Cartesian(0.0, speed, 0, gyro.getAngle());
    }
    public void driveBackward(double speed, Gyroscope gyro){
        drive.mecanumDrive_Cartesian(0.0, -speed, 0.0, gyro.getAngle());
    }
    public void strafeLeft(double speed, Gyroscope gyro){
        drive.mecanumDrive_Cartesian(-speed, 0.0, 0.0, gyro.getAngle());
    }
    public void strafeRight(double speed, Gyroscope gyro){
        drive.mecanumDrive_Cartesian(speed, 0.0, 0.0, gyro.getAngle());
    }
    public boolean turnRight(double speed, double angle, Gyroscope gyro){
        double targetAngle = angle;
        double startingAngle = gyro.getAngle();
        for(double gyroangle=startingAngle;gyroangle<startingAngle+targetAngle;gyroangle=gyro.getAngle()){
            drive.mecanumDrive_Cartesian(0.0, 0.0, speed, gyro.getAngle());
        }
        return true;
    }
    public boolean turnLeft(double speed, double angle, Gyroscope gyro){
        double targetAngle = angle;
        double startingAngle = gyro.getAngle();
        for(double gyroangle=startingAngle;gyroangle<startingAngle-targetAngle;gyroangle=gyro.getAngle()){
            drive.mecanumDrive_Cartesian(0.0, 0.0, -speed, gyro.getAngle());
        }
        return true;
        
    }
    
    public void driveWithJoysticks(Joystick stick, Gyro gyro){
        drive.mecanumDrive_Cartesian(stick.getX(), stick.getY(), stick.getZ(), gyro.getAngle());
    }
}
